#include "AlgoEddyTracker.hpp"
#include "AlgoEddyTrack.h"
#include <cmath>
#include <limits>
#include <algorithm>
#include <iostream>
#include <stdexcept>

#define NOMINMAX
#include "Poco/UUID.h"
#include "Poco/UUIDGenerator.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace EddyDetection {

    std::string gen_uuid() 
    {
        Poco::UUIDGenerator& generator = Poco::UUIDGenerator::defaultGenerator();
        Poco::UUID uuid = generator.create();
        return uuid.toString();
    }


    std::vector<std::string> get_times(const std::string& time_start, const std::string& time_end, const double frame_interval_hours) {
        std::vector<std::string> times;

        if (frame_interval_hours == 24.0 && time_start.size() == 8 && time_end.size() == 8) {
            // Parse the dates
            int start_year = std::stoi(time_start.substr(0, 4));
            int start_month = std::stoi(time_start.substr(4, 2));
            int start_day = std::stoi(time_start.substr(6, 2));

            int end_year = std::stoi(time_end.substr(0, 4));
            int end_month = std::stoi(time_end.substr(4, 2));
            int end_day = std::stoi(time_end.substr(6, 2));

            // Convert to tm struct for easier manipulation
            struct tm start_tm = { 0 };
            start_tm.tm_year = start_year - 1900;
            start_tm.tm_mon = start_month - 1;
            start_tm.tm_mday = start_day;

            struct tm end_tm = { 0 };
            end_tm.tm_year = end_year - 1900;
            end_tm.tm_mon = end_month - 1;
            end_tm.tm_mday = end_day;

            // Convert to time_t
            time_t start_time = mktime(&start_tm);
            time_t end_time = mktime(&end_tm);

            // Generate daily sequence
            for (time_t t = start_time; t <= end_time; t += 24 * 3600) {
                struct tm* tm_ptr = localtime(&t);
                char buffer[9];
                strftime(buffer, sizeof(buffer), "%Y%m%d", tm_ptr);
                times.push_back(buffer);
            }
        }

        return times;
    }

// 2D Euclidean distance calculation (for non-geographic coordinates)
double euclidean_distance_2d(const BoundaryPoint2D& p1, const BoundaryPoint2D& p2) {
    double dx = p1.x - p2.x;
    double dy = p1.y - p2.y;
    return std::sqrt(dx * dx + dy * dy);
}

    // Haversine distance calculation for geographic coordinates (latitude, longitude)
    double haversine_distance(const BoundaryPoint2D& p1, const BoundaryPoint2D& p2) {
        const double R = 6371.0; // Earth's radius in kilometers
    
        // Convert degrees to radians
        double lat1_rad = p1.x * M_PI / 180.0;
        double lon1_rad = p1.y * M_PI / 180.0;
        double lat2_rad = p2.x * M_PI / 180.0;
        double lon2_rad = p2.y * M_PI / 180.0;
    
        // Differences in coordinates
        double dlat = lat2_rad - lat1_rad;
        double dlon = lon2_rad - lon1_rad;
    
        // Haversine formula
        double a = std::sin(dlat/2) * std::sin(dlat/2) +
                   std::cos(lat1_rad) * std::cos(lat2_rad) *
                   std::sin(dlon/2) * std::sin(dlon/2);
        double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1-a));
    
        return R * c; // Distance in kilometers
    }

    // Complete implementation of KalmanFilter2D
    KalmanFilter2D::KalmanFilter2D(const BoundaryPoint2D& initial_pos, double dt_) : dt(dt_) {
        // Initialize state vector [lat, lon, vlat, vlon]
        x = {initial_pos.x, initial_pos.y, 0.0, 0.0};
    
        // Initialize matrices
        initialize_matrices();
    }

    void KalmanFilter2D::initialize_matrices() {
        // Initialize state covariance matrix P (4x4)
        P = std::vector<std::vector<double>>(4, std::vector<double>(4, 0.0));
        for (int i = 0; i < 2; ++i) {
            P[i][i] = 100.0;  // Position uncertainty
            P[i+2][i+2] = 10.0;  // Velocity uncertainty
        }
    
        // State transition matrix F (4x4)
        F = std::vector<std::vector<double>>(4, std::vector<double>(4, 0.0));
        for (int i = 0; i < 4; ++i) F[i][i] = 1.0;  // Diagonal is 1
        // Add position-velocity relationship
        F[0][2] = dt;  // x += vx * dt
        F[1][3] = dt;  // y += vy * dt
    
        // Observation matrix H (2x4) - we only observe position
        H = std::vector<std::vector<double>>(2, std::vector<double>(4, 0.0));
        H[0][0] = 1.0;  // Observe x
        H[1][1] = 1.0;  // Observe y
    
        // Process noise covariance matrix Q (4x4)
        Q = std::vector<std::vector<double>>(4, std::vector<double>(4, 0.0));
        double q = 1.0;  // Process noise intensity
        for (int i = 0; i < 2; ++i) {
            Q[i][i] = q * dt * dt / 4.0;      // Position noise
            Q[i+2][i+2] = q;                  // Velocity noise
            Q[i][i+2] = q * dt / 2.0;         // Position-velocity covariance
            Q[i+2][i] = q * dt / 2.0;         // Velocity-position covariance
        }
    
        // Measurement noise covariance matrix R (2x2)
        R = std::vector<std::vector<double>>(2, std::vector<double>(2, 0.0));
        double r = 10.0;  // Measurement noise intensity
        for (int i = 0; i < 2; ++i) R[i][i] = r;
    }

    void KalmanFilter2D::predict() {
        // Prediction step
        // x_pred = F * x
        std::vector<double> x_pred(4, 0.0);
        for (size_t i = 0; i < 4; ++i) {
            for (size_t j = 0; j < 4; ++j) {
                x_pred[i] += F[i][j] * x[j];
            }
        }
        x = x_pred;
    
        // P_pred = F * P * F^T + Q
        auto FP = matrix_multiply(F, P);
        auto FT = matrix_transpose(F);
        auto FPFT = matrix_multiply(FP, FT);
        P = matrix_add(FPFT, Q);
    }

    void KalmanFilter2D::update(const BoundaryPoint2D& measurement) {
        // Observation vector
        std::vector<double> z = {measurement.x, measurement.y};
    
        // Calculate innovation y = z - H * x
        std::vector<double> Hx(2, 0.0);
        for (size_t i = 0; i < 2; ++i) {
            for (size_t j = 0; j < 4; ++j) {
                Hx[i] += H[i][j] * x[j];
            }
        }
        std::vector<double> y(2);
        for (size_t i = 0; i < 2; ++i) {
            y[i] = z[i] - Hx[i];
        }
    
        // Calculate innovation covariance S = H * P * H^T + R
        auto HP = matrix_multiply(H, P);
        auto HT = matrix_transpose(H);
        auto HPHT = matrix_multiply(HP, HT);
        auto S = matrix_add(HPHT, R);
    
        // Calculate Kalman gain K = P * H^T * S^(-1)
        auto PHT = matrix_multiply(P, HT);
        auto S_inv = matrix_inverse(S);
        auto K = matrix_multiply(PHT, S_inv);
    
        // Update state x = x + K * y
        for (size_t i = 0; i < 4; ++i) {
            for (size_t j = 0; j < 2; ++j) {
                x[i] += K[i][j] * y[j];
            }
        }
    
        // Update covariance P = (I - K * H) * P
        auto KH = matrix_multiply(K, H);
        std::vector<std::vector<double>> I(4, std::vector<double>(4, 0.0));
        for (int i = 0; i < 4; ++i) I[i][i] = 1.0;
        auto IKH = matrix_subtract(I, KH);
        P = matrix_multiply(IKH, P);
    }

    BoundaryPoint2D KalmanFilter2D::get_position() const {
        return {x[0], x[1]};
    }

    BoundaryPoint2D KalmanFilter2D::get_velocity() const {
        return {x[2], x[3]};
    }

    BoundaryPoint2D KalmanFilter2D::predict_next_position() const {
        return {x[0] + x[2] * dt, x[1] + x[3] * dt};
    }

    void KalmanFilter2D::set_process_noise(double process_noise) {
        double q = process_noise;
        for (int i = 0; i < 2; ++i) {
            Q[i][i] = q * dt * dt / 4.0;
            Q[i+2][i+2] = q;
            Q[i][i+2] = q * dt / 2.0;
            Q[i+2][i] = q * dt / 2.0;
        }
    }

    void KalmanFilter2D::set_measurement_noise(double measurement_noise) {
        for (int i = 0; i < 2; ++i) {
            R[i][i] = measurement_noise;
        }
    }

    // State setting method implementation
    void KalmanFilter2D::set_state(const BoundaryPoint2D& position, const BoundaryPoint2D& velocity) {
        x[0] = position.x;
        x[1] = position.y;
        x[2] = velocity.x;
        x[3] = velocity.y;
    }

    void KalmanFilter2D::set_position(const BoundaryPoint2D& position) {
        x[0] = position.x;
        x[1] = position.y;
    }

    void KalmanFilter2D::set_velocity(const BoundaryPoint2D& velocity) {
        x[2] = velocity.x;
        x[3] = velocity.y;
    }

    // Get covariance matrix for uncertainty assessment
    const std::vector<std::vector<double>>& KalmanFilter2D::get_covariance_matrix() const {
        return P;
    }

    // Matrix operation helper function implementation
    std::vector<std::vector<double>> KalmanFilter2D::matrix_multiply(
        const std::vector<std::vector<double>>& A,
        const std::vector<std::vector<double>>& B) const {
    
        size_t rows_A = A.size();
        size_t cols_A = A[0].size();
        size_t cols_B = B[0].size();
    
        std::vector<std::vector<double>> result(rows_A, std::vector<double>(cols_B, 0.0));
    
        for (size_t i = 0; i < rows_A; ++i) {
            for (size_t j = 0; j < cols_B; ++j) {
                for (size_t k = 0; k < cols_A; ++k) {
                    result[i][j] += A[i][k] * B[k][j];
                }
            }
        }
    
        return result;
    }

    std::vector<std::vector<double>> KalmanFilter2D::matrix_add(
        const std::vector<std::vector<double>>& A,
        const std::vector<std::vector<double>>& B) const {
    
        size_t rows = A.size();
        size_t cols = A[0].size();
        std::vector<std::vector<double>> result(rows, std::vector<double>(cols));
    
        for (size_t i = 0; i < rows; ++i) {
            for (size_t j = 0; j < cols; ++j) {
                result[i][j] = A[i][j] + B[i][j];
            }
        }
    
        return result;
    }

    std::vector<std::vector<double>> KalmanFilter2D::matrix_subtract(
        const std::vector<std::vector<double>>& A,
        const std::vector<std::vector<double>>& B) const {
    
        size_t rows = A.size();
        size_t cols = A[0].size();
        std::vector<std::vector<double>> result(rows, std::vector<double>(cols));
    
        for (size_t i = 0; i < rows; ++i) {
            for (size_t j = 0; j < cols; ++j) {
                result[i][j] = A[i][j] - B[i][j];
            }
        }
    
        return result;
    }

    std::vector<std::vector<double>> KalmanFilter2D::matrix_transpose(
        const std::vector<std::vector<double>>& A) const {
    
        size_t rows = A.size();
        size_t cols = A[0].size();
        std::vector<std::vector<double>> result(cols, std::vector<double>(rows));
    
        for (size_t i = 0; i < rows; ++i) {
            for (size_t j = 0; j < cols; ++j) {
                result[j][i] = A[i][j];
            }
        }
    
        return result;
    }

    std::vector<std::vector<double>> KalmanFilter2D::matrix_inverse(
        const std::vector<std::vector<double>>& A) const {
    
        size_t n = A.size();
        std::vector<std::vector<double>> result(n, std::vector<double>(n, 0.0));
        std::vector<std::vector<double>> temp = A;
    
        // Create identity matrix
        for (size_t i = 0; i < n; ++i) {
            result[i][i] = 1.0;
        }
    
        // Gauss-Jordan elimination
        for (size_t i = 0; i < n; ++i) {
            // Find pivot
            double pivot = temp[i][i];
            if (std::abs(pivot) < 1e-10) {
                // Matrix is singular, return identity matrix as approximation
                for (size_t j = 0; j < n; ++j) {
                    for (size_t k = 0; k < n; ++k) {
                        result[j][k] = (j == k) ? 1.0 : 0.0;
                    }
                }
                return result;
            }
        
            // Normalize current row
            for (size_t j = 0; j < n; ++j) {
                temp[i][j] /= pivot;
                result[i][j] /= pivot;
            }
        
            // Elimination
            for (size_t j = 0; j < n; ++j) {
                if (i != j) {
                    double factor = temp[j][i];
                    for (size_t k = 0; k < n; ++k) {
                        temp[j][k] -= factor * temp[i][k];
                        result[j][k] -= factor * result[i][k];
                    }
                }
            }
        }
    
        return result;
    }

    // EddyTracker updated implementation
    EddyTracker::EddyTracker(double max_match_dist_, double dt_) 
        : max_match_dist(max_match_dist_), dt(dt_) {}

    void EddyTracker::init_tracks(const Frame& first_frame) {
        // Create trajectories for each eddy at each depth
        for (size_t layer_idx = 0; layer_idx < first_frame.layer_eddies.size(); ++layer_idx) {
            for (const auto& eddy : first_frame.layer_eddies[layer_idx]) {
                add_new_track_with_kalman(first_frame, eddy, layer_idx);
            }
        }
    }

    void EddyTracker::add_new_track_with_kalman(const Frame& frame, const EddyContourData& eddy, int layer_idx) {
        Track track;
        //track.track_id = next_track_id++;
        track.track_id = gen_uuid();
        track.eddies.emplace_back(frame.frame_idx, eddy);
        track.depth = eddy.depth;
        track.layer_index = layer_idx;
        track.time_start = frame.time;
        tracks.push_back(track);
    
        // Create 2D Kalman filter for new trajectory
        auto kalman = std::make_unique<KalmanFilter2D>(eddy.center, dt);
        kalman->set_process_noise(process_noise);
        kalman->set_measurement_noise(measurement_noise);
    
        //std::cout << "Created Kalman filter for the: " << track.track_id << " track" << std::endl;
        kalman_filters.push_back(std::move(kalman));
    }

    void EddyTracker::match_and_update(const Frame& frame) {
        // Match for each depth layer separately
        for (size_t layer_idx = 0; layer_idx < frame.layer_eddies.size(); ++layer_idx) {
            std::vector<int> used(frame.layer_eddies[layer_idx].size(), 0);
            //std::cout << "The " << layer_idx << " layer has " << used.size() << " eddies" << std::endl;
        
            // Match trajectories at current depth
            for (size_t track_idx = 0; track_idx < tracks.size(); ++track_idx) {
                auto& track = tracks[track_idx];
                if (!track.alive || track.layer_index != static_cast<int>(layer_idx)) continue;
            
                const EddyContourData& last_eddy = track.eddies.back().second;
            
                // Use Kalman filter to predict next position
                BoundaryPoint2D predicted_pos = last_eddy.center;
                if (track_idx < kalman_filters.size() && kalman_filters[track_idx]) {
                    kalman_filters[track_idx]->predict();
                    predicted_pos = kalman_filters[track_idx]->predict_next_position();
                }
            
                double min_dist = std::numeric_limits<double>::max();
                int min_idx = -1;
                //std::string mess = "min_dist is bigger than 50km";
            
                // Search for the best matching eddy
                for (size_t i = 0; i < frame.layer_eddies[layer_idx].size(); ++i) {
                    if (used[i]) continue;

                    // Type matching check
                    if (last_eddy.type != frame.layer_eddies[layer_idx][i].type) {
                        //mess = "type is not match";
                        continue;
                    }

                    // Area similarity check
                    double area_ratio = std::abs(last_eddy.area_km2 - frame.layer_eddies[layer_idx][i].area_km2) /
                        std::max(last_eddy.area_km2, frame.layer_eddies[layer_idx][i].area_km2);
                    if (area_ratio > 0.5) {  // Area change exceeds 50%, consider no match
                        //mess = "area_ratio is big than 0.5";
                        continue;
                    }
                
                    // Calculate distance using predicted position (Haversine for geographic coordinates)
                    double dist = haversine_distance(predicted_pos, frame.layer_eddies[layer_idx][i].center);
                
                    if (dist < min_dist) {
                        min_dist = dist;
                        min_idx = static_cast<int>(i);
                    }
                }
            
                if (min_dist < max_match_dist && min_idx != -1) {
                    update_track_with_kalman(track_idx, frame.layer_eddies[layer_idx][min_idx], frame.frame_idx);
                    used[min_idx] = 1;
                    //std::cout << "Track" << track_idx << " is continue " << std::endl;
                } else {
                    track.alive = false;
                    tracks[track_idx].time_end = frame.time;
                    //std::cout <<"Track"<< track_idx << " is dead: " << mess << std::endl;
                }
            }
        
            // Create new trajectories for unmatched eddies
            for (size_t i = 0; i < frame.layer_eddies[layer_idx].size(); ++i) {
                if (!used[i]) {
                    add_new_track_with_kalman(frame, frame.layer_eddies[layer_idx][i], layer_idx);
                }
            }
        }
    }

    void EddyTracker::update_track_with_kalman(size_t track_idx, const EddyContourData& eddy, int frame_idx) {
        tracks[track_idx].eddies.emplace_back(frame_idx, eddy);
    
        // Update corresponding Kalman filter
        if (track_idx < kalman_filters.size() && kalman_filters[track_idx]) {
            kalman_filters[track_idx]->update(eddy.center);
        }
        //std::cout << "kalman_filters.size, tracks.size = " << kalman_filters.size() << ", " << tracks.size() << std::endl;
    }

    void EddyTracker::kalman_update(const Frame& frame) {
        // This function is now mainly used for cleaning dead trajectories
        remove_dead_tracks(frame);
    }

    void EddyTracker::remove_dead_tracks(const Frame& frame) {
        // A filter that only releases death trajectories
        for (size_t i = 0; i < tracks.size(); ++i) {
            if (!tracks[i].alive && i < kalman_filters.size()) {
                kalman_filters[i].reset();
            }
        }
    }

    void EddyTracker::set_kalman_parameters(double process_noise_, double measurement_noise_) {
        process_noise = process_noise_;
        measurement_noise = measurement_noise_;
    
        // Update all existing Kalman filters
        for (auto& filter : kalman_filters) {
            if (filter) {
                filter->set_process_noise(process_noise);
                filter->set_measurement_noise(measurement_noise);
            }
        }
    }

    void EddyTracker::handle_split_merge() {
        // Handle complex logic for eddy splitting and merging
        // Based on Kalman filter predictions, these situations can be better handled
    
        // Handle merges first, then splits
        detect_and_handle_merges(Frame{}); // Pass empty frame, only based on trajectory state
        detect_and_handle_splits(Frame{}); // Pass empty frame, only based on trajectory state
    }

    void EddyTracker::set_split_merge_parameters(double merge_distance_ratio_, 
                                               double split_area_threshold_,
                                               double merge_area_threshold_) {
        merge_distance_ratio = merge_distance_ratio_;
        split_area_threshold = split_area_threshold_;
        merge_area_threshold = merge_area_threshold_;
    }

    void EddyTracker::detect_and_handle_merges(const Frame& current_frame) {
        std::vector<std::pair<size_t, size_t>> merge_candidates;
    
        // Detect all possible merge candidates (only within same depth layer)
        for (size_t i = 0; i < tracks.size(); ++i) {
            if (!tracks[i].alive) continue;
            for (size_t j = i + 1; j < tracks.size(); ++j) {
                if (!tracks[j].alive) continue;
                // Trajectories with a length less than 2 are not involved in the merging process
                if (tracks[i].eddies.size() < 2 || tracks[j].eddies.size() < 2) continue;
                // Only check trajectories in same depth layer
                if (tracks[i].layer_index != tracks[j].layer_index) continue;
            
                if (check_merge_criteria(i, j, current_frame)) {
                    merge_candidates.emplace_back(i, j);
                }
            }
        }
    
        // Execute merges (sort by distance, prioritize closest ones)
        std::sort(merge_candidates.begin(), merge_candidates.end(),
            [this](const std::pair<size_t, size_t>& a, const std::pair<size_t, size_t>& b) {
                if (a.first >= kalman_filters.size() || a.second >= kalman_filters.size() ||
                    b.first >= kalman_filters.size() || b.second >= kalman_filters.size() ||
                    !kalman_filters[a.first] || !kalman_filters[a.second] ||
                    !kalman_filters[b.first] || !kalman_filters[b.second]) {
                    return false;
                }
            
                auto pos_a1 = kalman_filters[a.first]->predict_next_position();
                auto pos_a2 = kalman_filters[a.second]->predict_next_position();
                auto pos_b1 = kalman_filters[b.first]->predict_next_position();
                auto pos_b2 = kalman_filters[b.second]->predict_next_position();
            
                double dist_a = haversine_distance(pos_a1, pos_a2);
                double dist_b = haversine_distance(pos_b1, pos_b2);
            
                return dist_a < dist_b;
            });
    
        // Execute merge operations
        for (const auto& candidate : merge_candidates) {
            size_t i = candidate.first;
            size_t j = candidate.second;
        
            if (!tracks[i].alive || !tracks[j].alive) continue; // Already processed by other merge operations
        
            // Decide which trajectory to keep (based on trajectory quality and area)
            double confidence_i = calculate_track_confidence(i);
            double confidence_j = calculate_track_confidence(j);
            std::cout << "Confidence of track " << i << " and " << j << " == " << confidence_i << " & " << confidence_j << std::endl;
        
            const auto& eddy_i = tracks[i].eddies.back().second;
            const auto& eddy_j = tracks[j].eddies.back().second;
        
            size_t keep_track, remove_track;
            double confidence_keep, confidence_remove;
            if (confidence_i > confidence_j || 
                (std::abs(confidence_i - confidence_j) < 0.1 && eddy_i.area_km2 > eddy_j.area_km2)) {
                keep_track = i; 
                confidence_keep = confidence_i;
                remove_track = j;
                confidence_remove = confidence_j;
            } else {
                keep_track = j;
                confidence_keep = confidence_j;
                remove_track = i;
                confidence_remove = confidence_i;
            }
        
            tracks[remove_track].time_end = current_frame.time;
            execute_merge(keep_track, remove_track, confidence_keep, confidence_remove);
        }
    }

    void EddyTracker::detect_and_handle_splits(const Frame& current_frame) {
        std::vector<std::pair<size_t, std::vector<std::pair<int, int>>>> split_candidates;
    
        // Detect possible split situations
        for (size_t i = 0; i < tracks.size(); ++i) {
            if (!tracks[i].alive || tracks[i].eddies.size() < 2) continue;
        
            std::vector<std::pair<int, int>> potential_splits;
            if (check_split_criteria(i, current_frame, potential_splits)) {
                split_candidates.emplace_back(i, potential_splits);
            }
        }
    
        // Execute split operations
        for (const auto& candidate : split_candidates) {
            size_t track_idx = candidate.first;
            const std::vector<std::pair<int, int>>& split_eddy_indices = candidate.second;
        
            if (!tracks[track_idx].alive) continue;
        
            // Execute split
            execute_split(track_idx, split_eddy_indices, current_frame);
        }
    }

    bool EddyTracker::check_merge_criteria(size_t track_i, size_t track_j, const Frame& current_frame) {
        // Basic condition check
        if (track_i >= kalman_filters.size() || track_j >= kalman_filters.size() ||
            !kalman_filters[track_i] || !kalman_filters[track_j]) {
            return false;
        }
    
        const auto& eddy_i = tracks[track_i].eddies.back().second;
        const auto& eddy_j = tracks[track_j].eddies.back().second;
    
        // 1. Types must be the same
        if (eddy_i.type != eddy_j.type) {
            return false;
        }
    
        // 2. Predicted position distance check
        auto pos_i = kalman_filters[track_i]->predict_next_position();
        auto pos_j = kalman_filters[track_j]->predict_next_position();
        double dist = haversine_distance(pos_i, pos_j);
    
        if (dist > max_match_dist * merge_distance_ratio) {
            return false;
        }
    
        // 3. Area similarity check
        double area_ratio = std::abs(eddy_i.area_km2 - eddy_j.area_km2) / 
                           std::max(eddy_i.area_km2, eddy_j.area_km2);
        if (area_ratio > merge_area_threshold) {
            return false;
        }
    
        // 4. Motion direction consistency check
        if (tracks[track_i].eddies.size() >= 2 && tracks[track_j].eddies.size() >= 2) {
            auto vel_i = kalman_filters[track_i]->get_velocity();
            auto vel_j = kalman_filters[track_j]->get_velocity();
        
            // Calculate angle between velocity vectors
            double dot_product = vel_i.x * vel_j.x + vel_i.y * vel_j.y;
            double mag_i = std::sqrt(vel_i.x*vel_i.x + vel_i.y*vel_i.y);
            double mag_j = std::sqrt(vel_j.x*vel_j.x + vel_j.y*vel_j.y);
        
            if (mag_i > 0.01 && mag_j > 0.01) {
                double cos_angle = dot_product / (mag_i * mag_j);
                if (cos_angle < 0.5) { // Angle greater than 60 degrees, significant motion direction difference
                    return false;
                }
            }
        }
    
        // 5. Historical trajectory proximity check
        if (tracks[track_i].eddies.size() >= 3 && tracks[track_j].eddies.size() >= 3) {
            // Check if recent frames' trajectories are gradually approaching
            bool approaching = true;
            for (size_t k = 1; k <= std::min(size_t(3), std::min(tracks[track_i].eddies.size() - 1, tracks[track_j].eddies.size() - 1)); ++k) {
                auto pos_i_prev = tracks[track_i].eddies[tracks[track_i].eddies.size()-1-k].second.center;
                auto pos_j_prev = tracks[track_j].eddies[tracks[track_j].eddies.size()-1-k].second.center;
                double dist_prev = haversine_distance(pos_i_prev, pos_j_prev);
            
                if (dist_prev <= dist) {  // Distance not decreasing
                    approaching = false;
                    break;
                }
            }
        
            if (!approaching) {
                return false;
            }
        }
    
        return true;
    }

    // Enhanced split detection function
    bool EddyTracker::check_split_criteria(size_t track_idx, const Frame& current_frame, 
                                         std::vector<std::pair<int, int>>& potential_splits) {
        if (tracks[track_idx].eddies.size() < 2) {
            return false;
        }
    
        const auto& current_eddy = tracks[track_idx].eddies.back().second;
        const auto& prev_eddy = tracks[track_idx].eddies[tracks[track_idx].eddies.size()-2].second;
    
        bool split_detected = false;
    
        // 1. Check for area mutation
        double area_ratio = current_eddy.area_km2 / prev_eddy.area_km2;
        if (area_ratio > split_area_threshold) {
            split_detected = true;
            std::cout << "Area-based split detected: ratio = " << area_ratio << std::endl;
        }
    
        // 2. Check for prediction error spike
        if (track_idx < kalman_filters.size() && kalman_filters[track_idx]) {
            double prediction_error = calculate_prediction_error(track_idx, current_eddy.center);
        
            if (prediction_error > max_match_dist * 0.8) {
                split_detected = true;
                std::cout << "Prediction error-based split detected: error = " << prediction_error << std::endl;
            }
        }
    
        // 3. Check spatial separation (if current frame data exists)
        if (!current_frame.layer_eddies.empty()) {
            int layer_idx = tracks[track_idx].layer_index;
            if (layer_idx < static_cast<int>(current_frame.layer_eddies.size())) {
                // Look for other eddies that might be related to current trajectory
                for (size_t i = 0; i < current_frame.layer_eddies[layer_idx].size(); ++i) {
                    const auto& frame_eddy = current_frame.layer_eddies[layer_idx][i];
                
                    // Check if it might be a split eddy
                    if (frame_eddy.type == current_eddy.type && 
                        frame_eddy.id != current_eddy.id) {
                    
                        double dist = haversine_distance(current_eddy.center, frame_eddy.center);
                    
                        // If distance is close and area is similar, might be split result
                        if (dist < max_match_dist * 0.5) {
                            double area_ratio = std::abs(current_eddy.area_km2 - frame_eddy.area_km2) / 
                                            std::max(current_eddy.area_km2, frame_eddy.area_km2);
                        
                            if (area_ratio < 0.3) {  // Area similar
                                potential_splits.push_back({layer_idx, static_cast<int>(i)});
                                split_detected = true;
                                std::cout << "Spatial split candidate found: distance = " << dist 
                                          << ", area ratio = " << area_ratio << std::endl;
                            }
                        }
                    }
                }
            }
        }
    
        return split_detected;
    }

    void EddyTracker::execute_merge(size_t keep_track, size_t remove_track, double confidence_keep, double confidence_remove) {
        // Mark trajectory to be removed as dead
        tracks[remove_track].alive = false;
    
        std::cout << "Executing merge: keeping track " << tracks[keep_track].track_id 
                  << ", removing track " << tracks[remove_track].track_id << std::endl;
    
        // Fuse information from two Kalman filters
        // Get states from two filters
        auto pos_keep = kalman_filters[keep_track]->get_position();
        auto pos_remove = kalman_filters[remove_track]->get_position();
        auto vel_keep = kalman_filters[keep_track]->get_velocity();
        auto vel_remove = kalman_filters[remove_track]->get_velocity();
    
        // Calculate fusion weights (based on trajectory confidence)
        double total_confidence = confidence_keep + confidence_remove;
    
        if (total_confidence > 0) {
            double weight_keep = confidence_keep / total_confidence;
            double weight_remove = confidence_remove / total_confidence;
        
            // Fuse position and velocity
            BoundaryPoint2D fused_pos = {
                pos_keep.x * weight_keep + pos_remove.x * weight_remove,
                pos_keep.y * weight_keep + pos_remove.y * weight_remove
            };
        
            BoundaryPoint2D fused_vel = {
                vel_keep.x * weight_keep + vel_remove.x * weight_remove,
                vel_keep.y * weight_keep + vel_remove.y * weight_remove
            };
        
            // Update Kalman filter state of kept trajectory
            kalman_filters[keep_track]->set_state(fused_pos, fused_vel);
        }
    
        // Record merge history (optional)
        // Here you can add recording of merge events for subsequent analysis
    }

    void EddyTracker::execute_split(size_t parent_track, const std::vector<std::pair<int, int>>& split_eddy_indices, 
                                   const Frame& current_frame) {
        if (split_eddy_indices.empty()) {
            return;
        }
    
        std::cout << "Split detected for track " << tracks[parent_track].track_id 
                  << " into " << split_eddy_indices.size() << " parts" << std::endl;
    
        // Get last eddy of parent trajectory
        const auto& parent_eddy = tracks[parent_track].eddies.back().second;
    
        // Create new trajectories for each split eddy
        for (const auto& split_info : split_eddy_indices) {
            int layer_idx = split_info.first;
            int eddy_idx = split_info.second;
        
            if (layer_idx < static_cast<int>(current_frame.layer_eddies.size()) && 
                eddy_idx < static_cast<int>(current_frame.layer_eddies[layer_idx].size())) {
            
                const auto& split_eddy = current_frame.layer_eddies[layer_idx][eddy_idx];
            
                // Create new trajectory
                Track new_track;
                //new_track.track_id = next_track_id++;
                new_track.track_id = gen_uuid();
                new_track.eddies.emplace_back(current_frame.frame_idx, split_eddy);
                new_track.depth = split_eddy.depth;
                new_track.layer_index = layer_idx;
                tracks.push_back(new_track);
            
                // Create Kalman filter for new trajectory
                auto kalman = std::make_unique<KalmanFilter2D>(split_eddy.center, dt);
                kalman->set_process_noise(process_noise);
                kalman->set_measurement_noise(measurement_noise);
                kalman_filters.push_back(std::move(kalman));
            
                std::cout << "  Created new track " << new_track.track_id 
                          << " for split eddy " << split_eddy.id << std::endl;
            }
        }
    
        // Update parent trajectory (can choose to keep or mark as dead)
        // Here we choose to keep parent trajectory, let it continue tracking main eddy part
        // If needed, can mark parent trajectory as dead
        // tracks[parent_track].alive = false;
    }

    double EddyTracker::calculate_track_confidence(size_t track_idx) const {
        // Calculate confidence based on trajectory length and Kalman filter uncertainty
        double length_score = std::min(5.0, static_cast<double>(tracks[track_idx].eddies.size())) / 5.0;
    
        // Calculate position uncertainty using covariance matrix trace
        // Get the covariance matrix P from Kalman filter
        const auto& P = kalman_filters[track_idx]->get_covariance_matrix();
    
        // Calculate trace of position covariance (P[0][0] + P[1][1])
        double position_variance = P[0][0] + P[1][1];
        std::cout << "*******************************track "<<track_idx<<": P[0][0],P[1][1]==" << P[0][0]<<","<<P[1][1] << std::endl;
    
        // Scale factor for uncertainty score calculation
        // Based on typical position variance in geographic coordinates:
        // - Initial P[i][i] = 100.0 (degrees^2)
        // - Typical measurement noise R[i][i] = 10.0 (degrees^2)
        // - After convergence, position variance typically ranges 1-50 (degrees^2)
        // - Scale factor should be ~2-5 times the typical variance for good sensitivity
        double scale_factor = 50.0; // Adjust based on your data characteristics
    
        // Convert variance to uncertainty score (lower variance = higher confidence)
        // Use exponential decay: exp(-variance/scale_factor)
        double uncertainty_score = std::exp(-position_variance / scale_factor);
    
        // Ensure uncertainty score is in [0, 1] range
        uncertainty_score = std::max(0.0, std::min(1.0, uncertainty_score));
        std::cout << "uncertainty score of track " << track_idx << " = " << uncertainty_score << std::endl;
    
        return length_score * uncertainty_score;
    }

    double EddyTracker::calculate_prediction_error(size_t track_idx, const BoundaryPoint2D& actual_pos) const {
        if (track_idx >= kalman_filters.size() || !kalman_filters[track_idx]) {
            return std::numeric_limits<double>::max();
        }
    
        auto predicted_pos = kalman_filters[track_idx]->predict_next_position();
        return haversine_distance(predicted_pos, actual_pos);
    }

    double EddyTracker::haversine_distance(const BoundaryPoint2D& p1, const BoundaryPoint2D& p2) const {
        return calculate_haversine_distance(p1, p2);
    }

    double EddyTracker::calculate_haversine_distance(const BoundaryPoint2D& p1, const BoundaryPoint2D& p2) {
        const double R = 6371.0; // Earth's radius in kilometers
    
        // Convert degrees to radians
        double lat1_rad = p1.x * M_PI / 180.0;
        double lon1_rad = p1.y * M_PI / 180.0;
        double lat2_rad = p2.x * M_PI / 180.0;
        double lon2_rad = p2.y * M_PI / 180.0;
    
        // Differences in coordinates
        double dlat = lat2_rad - lat1_rad;
        double dlon = lon2_rad - lon1_rad;
    
        // Haversine formula
        double a = std::sin(dlat/2) * std::sin(dlat/2) +
                   std::cos(lat1_rad) * std::cos(lat2_rad) *
                   std::sin(dlon/2) * std::sin(dlon/2);
        double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1-a));
    
        return R * c; // Distance in kilometers
    }

    std::vector<Track> EddyTracker::get_tracks() const {
        return tracks;
    } 

    std::vector<Track> EddyDynamic::startDynamic(const std::vector<std::pair<std::string, VelocityData>>& velocity_datas,
                                                 const std::vector<std::pair<std::string, TemperatureData>>& temp_datas,
                                                const EddyDynamicConfig& config, const EddyDetectionConfig& detectconfig) {
        if (velocity_datas.size() != temp_datas.size()) {
            throw std::invalid_argument("The Sizes of Velocity_datas and Temp_datas are not Match");
        }
        std::vector<Frame> frames;

        for (size_t i = 0; i < velocity_datas.size(); i++) {
            Frame frame;

            Eddy ed;
            auto eddies2D = ed.star2D(velocity_datas[i].second, temp_datas[i].second, detectconfig);

            for (const auto eddies2D : eddies2D) {
                frame.depths.push_back(eddies2D.first);
                frame.layer_eddies.push_back(eddies2D.second);
            }
            frame.frame_idx = i;
            frame.time = velocity_datas[i].first;

            frames.push_back(frame);
        }

        std::cout << "Processing " << frames.size() << " frames..." << std::endl;

        double dt_days = config.frame_interval_hours / 24.0;
        double max_match_dist_km = config.daily_match_dist_km * dt_days;

        // Initialize tracker
        EddyTracker tracker(max_match_dist_km, dt_days);
        tracker.set_kalman_parameters(config.process_noise, config.meassurement_noise);
        //tracker.set_split_merge_parameters(config.merge_distance_ratio, config.split_area_threshold, config.merge_area_threshold);

        // Process all frames
        for (size_t i = 0; i < frames.size(); ++i) {
            if (i % 1 == 0) {
                std::cout << "Processing frame " << i << "/" << frames.size() << std::endl;
            }

            if (i == 0) {
                tracker.init_tracks(frames[i]);
            }
            else {
                tracker.match_and_update(frames[i]);
            }

            //// Handle split-merge every few frames
            //if (i % 5 == 0) {
            //    tracker.handle_split_merge();
            //}

            tracker.kalman_update(frames[i]);
        }

        // Get final tracks
        std::vector<Track> tracks = tracker.get_tracks();
        std::cout << "Final tracks: " << tracks.size() << std::endl;
        std::cout << "Final active tracks: " << std::count_if(tracks.begin(), tracks.end(),
            [](const Track& t) { return t.alive; }) << std::endl;

        return tracks;
    }

} // namespace EddyDetection 
